﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;

using System.Text;
using System.Windows.Forms;
using System.Drawing.Drawing2D;

using System.Windows;

namespace AI
{
    public partial class AIInfo
    {



        DateTime _UpdateTime = DateTime.Now;

        public UrbanSignalState _Signal = null;

        public void SetRGCount(UrbanSignalState Signal)
        {
            

            if (_LastCount == 2)
            {
                _MyCar.MoveCar(_mainPort,0.1d, 0.1d);
                return;

            }

            if (bFirstCheckDir == true)
                return;

            TimeSpan span = DateTime.Now - _UpdateTime;

            _UpdateTime = DateTime.Now;

            if (span.TotalMilliseconds < 0.1d)
            {
                AddLog("update return");
                return;
            }
            
            _Signal = Signal;
            this.labelRSignal.Text = "R :" + _Signal.RCount.ToString();
            this.labelGSignal.Text = "G :" + _Signal.GCount.ToString();
            this.labelDrive.Text = "LEFT : " + _Signal.LeftCount.ToString() + "-" + "RIGHT : " + _Signal.RightCount.ToString();

            //_MyCar.UpdateDiffInfo(span.TotalMilliseconds);



            if (Vector.Subtract(_MyCar.Pos, _MovePoint).Length < _MoveRange)
            {
                //if (_NextPoint.Range == 0.00001d)
                //{
                //    MoveCar(0.0d, 0.0d);
                //    return;
                //}
                //else
                {

                    NextPoint();
                    if (_LastCount == 1)
                    {
                        _LastCount++;
                        return;
                    }

                }

            }

            if (_NextPoint == null)
            {
                // 마지막에는 목적지로.
                _NextPoint = new Point();
                _NextPoint._GPSPos = _DestInfo.EndPoint;
                _MovePoint = _NextPoint._GPSPos;

                _MoveRange = 0.00001d;

                _LastCount++;
                //return;
            }
            else
            {
            }




            if (0 < _SignalCheckCount)
            {

                AddLog("Check signal");

                if (_Signal.RCount == uint.MaxValue)
                {
                    //AddLog("wait signal 1");
                    //return;
                }

                if (0 < _Signal.RCount && 0 < _Signal.GCount)
                {
                    //AddLog("wait signal 2");
                    //return;
                }


                if (1 < _Signal.RCount)
                {
                    // 처음 시작할때 바로 앞에 신호등을 검색해서 빨간색이면 멈춤

                    _MyCar.MoveCar(_mainPort,0, 0);
                    _SignalCheckCount--;
                    AddLog("Check red");
                    return;

                }

            }

            //비전으로 위점지역으로 간다고 판단되면
            // 128*128/2/2 * 2/3 74
            //128*54/2/3 = 1152
            // 위험지역 2/3가 넘어가면 

            int Threshold = 1152;

            if (Threshold * 2 < _Signal.LeftCount && _Signal.RightCount < Threshold * 2)
            {

                _MyCar.MoveCar(_mainPort,0.5d, 0.35d);

                AddLog("move force right");

                return;
            }
            else if (Threshold * 2 < _Signal.RightCount && _Signal.LeftCount < Threshold * 2)
            {

                _MyCar.MoveCar(_mainPort,0.35d, 0.5d);
                AddLog("move force left");
                return;
            }


            Vector _TargetDir = _MovePoint - _MyCar.Pos;

            _TargetDir = _MIM._BaseNode.GetDir(_TargetDir.X, _TargetDir.Y);

            //AddLog("target : " + _MovePoint.ToString());
            //AddLog("cur  : " + _MyCar.Pos.ToString());

            double rad = Vector.Multiply(_MyCar._Dir, _TargetDir);
            double angle = Math.Acos(rad) * 180 / Math.PI;

            double vel = 0.5;
            double speed = 0.5;

            if (angle < 90)
            {
                vel = 1.0d - (0.8d * angle / 90.0d);
                speed = 1.0 - (0.5 * angle / 90);
            }

            float carDegree = MathUtil.GetDegreesForBaseRotation((float)_MyCar._Dir.X, (float)_MyCar._Dir.Y);
            float nextTargetDegree = MathUtil.GetDegreesForBaseRotation((float)_TargetDir.X, (float)_TargetDir.Y);

            int bRight;

            if (carDegree < nextTargetDegree)
                bRight = 1;
            else
                bRight = -1;

            if (180 < Math.Abs(carDegree - nextTargetDegree))
            {
                bRight *= -1;

            }

            if (bRight == -1)
            {
                double left = (double)1 * speed * _NodeSpeed * 0.5d;
                double right = vel * speed * _NodeSpeed * 0.5d;

                _MyCar.MoveCar(_mainPort, left, right);

                
            }
            else
            {
                double left = vel * speed * _NodeSpeed * 0.5d;
                double right = (double)1 * speed * _NodeSpeed * 0.5d;
                _MyCar.MoveCar(_mainPort, left, right);
                
            }

            

        }


        private void timer2_Tick(object sender, EventArgs e)
        {
            return;

            //if (checkBoxMoveStop.Checked)
            //    return;

            //if (_NextPoint == null)
            //    return;

            ////_MyCar.UpdateMove();
            //if (Vector.Subtract(_MyCar.Pos, _NextPoint._GPSPos).Length < 0.0002)
            //{
            //    _NextPoint = _MIM.GetNextPoint();
            //    //AddLog(_NextPoint);
            //}

            //if (_NextPoint == null)
            //    return;

            //Vector _TargetDir = _NextPoint._GPSPos - _MyCar.Pos;

            //_TargetDir = _MIM._BaseNode.GetDir(_TargetDir.X, _TargetDir.Y);

            //double velocity = Vector.Multiply(_TargetDir, _MyCar._Dir) / 4 + 0.2;

            //float carDegree = MathUtil.GetDegreesForBaseRotation((float)_MyCar._Dir.X, (float)_MyCar._Dir.Y);
            //float nextTargetDegree = MathUtil.GetDegreesForBaseRotation((float)_TargetDir.X, (float)_TargetDir.Y);

            //int bRight;

            //if (carDegree < nextTargetDegree)
            //    bRight = 1;
            //else
            //    bRight = -1;

            //if (180 < Math.Abs(carDegree - nextTargetDegree))
            //{
            //    bRight *= -1;
            //}

            //DriveInfo body2 = new DriveInfo();

            //if (bRight == -1)
            //{
            //    body2.left = 0.45;
            //    body2.right = velocity;
            //}
            //else
            //{
            //    body2.left = velocity;
            //    body2.right = 0.45;
            //}


            //_mainPort.Post(new NotifyDrive(body2));

        }
    }
}
